/*
 * message.c
 *
 *  Created on: 05/11/2012
 *      Author: julian
 */

#include <stdlib.h>
#include <stdio.h>
#include <string.h>
#include <pthread.h>
#include <time.h>
#include <sys/time.h>

#include "message.h"
#include "packet_output.h"
#include "posix_mq_utils.h"
#include "ABP.h"
#include "app.h"

static pthread_key_t fid_var_key;
static pthread_once_t key_once = PTHREAD_ONCE_INIT;

static void make_key() {
    pthread_key_create (&fid_var_key, NULL);
}

void messageInit() {

    pthread_once(&key_once, make_key);
    abpInit();

}

void msgFidSpecific(uint8_t *fid) {

    pthread_setspecific (fid_var_key, fid);

}

void print_msg_buf(struct msg_buf cbuf) {

    uint8_t i = 0;
    printf("\nmsg_buf type:%X ", cbuf.mtype);
    printf("opts: %X ", cbuf.msg.options);
    printf("action: %X ", cbuf.msg.type);
    printf("len: %d ", cbuf.msg.len);
    printf("nodeId: %s ", cbuf.msg.nodeId);
    printf("data: ");

    for (i = 0; i < cbuf.msg.len; i++) {
        printf("%X", cbuf.msg.data[i]);
    }
    printf("\n");
}

void print_control_msg_buf(struct control_msg_buf cbuf) {

    printf("\ncmsg_buf type:%X ", cbuf.mtype);
    printf("opts: %X ", cbuf.cmsg.options);
    printf("action: %X ", cbuf.cmsg.type);
    printf("status: %d ", cbuf.cmsg.status);
}

void print_msg_t(struct msg_t msg) {

    uint8_t i = 0;
    printf("\n msg_t:opts: %X ", msg.options);
    printf("action: %X ", msg.type);
    printf("len: %d ", msg.len);
    printf("nodeId: %s ", msg.nodeId);
    printf("data: ");
    for (i = 0; i < msg.len; i++) {
        printf("%X", msg.data[i]);
    }
    printf("\n");
}

static int packetizeMsg_16(struct msg_t *msg, xbee_TxFrame16_t *sending16,
        uint16_t addr16) {

    uint8_t checksum = 0, i = 0;

    sending16->dataSize = msg->len + 3;

    sending16->destinationAddress = addr16;

    sending16->options = 0x00;
    sending16->data = malloc(sending16->dataSize * sizeof(uint8_t));

    sending16->data[0] = msg->options;
    sending16->data[1] = msg->type;

    for (i = 0; i < msg->len; i++) {
        sending16->data[2 + i] = (uint8_t) msg->data[i];
    }

    for (i = 0; i < sending16->dataSize - 1; i++) {
        checksum += sending16->data[i];
    }
    sending16->data[sending16->dataSize - 1] = 0xff - checksum;

    return 0;

}

static int packetizeMsg_64(struct msg_t *msg, xbee_TxFrame64_t *sending64,
        uint64_t addr64) {

    uint8_t checksum = 0, i = 0;

    sending64->dataSize = msg->len + 3;
    sending64->destinationAddress = addr64;

    sending64->options = 0x00;
    sending64->data = malloc(sending64->dataSize * sizeof(uint8_t));

    sending64->data[0] = msg->options;
    sending64->data[1] = msg->type;

    for (i = 0; i < msg->len; i++) {
        sending64->data[2 + i] = (uint8_t) msg->data[i];
    }

    for (i = 0; i < sending64->dataSize - 1; i++) {
        checksum += sending64->data[i];
    }
    sending64->data[sending64->dataSize - 1] = 0xff - checksum;

    return 0;

}

int msgOutput(struct msg_t *msg) {

    if (msg == NULL || msg->nodeId== NULL) {
        errno = EINVAL;
        return -1;
    }

#ifdef MSG_DEBUG
        printf("\nmsgOutput: ");
        print_msg_t(*msg);
#endif

    if (strcmp(msg->nodeId, BROADCAST_ID) == 0) {
        msg->options |= BROADCAST_BIT;
    }

    if (msg->options & BROADCAST_BIT) {
        //broadcast
        if (sendPacket(msg, 0xFFFF, 0) == -1) {
#ifdef MSG_DEBUG
            fprintf(stderr,"msgOutput:sendPacket.Error al enviar mensaje");
#endif
            return (-1);
        } else {
            return 0;
        }
    } else {

        wpan_node_t *tmp = (wpan_node_t*)findNodeById(msg->nodeId);
        if (tmp == NULL) {
            //errno=EINVAL;
            return (-1);
        } else {

            if (abpOut(msg, tmp) == -1) {
#ifdef MSG_DEBUG
            fprintf(stderr,"msgOutput:abpOut.Error al enviar mensaje");
#endif
                return (-1);
            }

        }
        return (0);

    }
}

int msgInput(int node_msqid, char *identifier, uint8_t *data, uint8_t size) {

    int i = 0;
    uint8_t chksum = 0;

    for (i = 0; i < size; i++) {
        chksum += data[i];
    }

    if (chksum != 0xFF) {
        return (-1);
    } else {

        struct msg_buf cbuf;
        struct control_msg_buf cmbuf;
        memset(&cmbuf, 0, sizeof(struct control_msg_buf));
        memset(&cbuf, 0, sizeof(struct msg_buf));

//        if (data[2] > MAX_MSG_DATA_LEN) {
//            return (-1);
//        }

        if (size > (MAX_MSG_LEN)) {
            return (-1);
        }

        switch (data[1]) {
        case ACK:
            cmbuf.cmsg.options = data[0];
            cmbuf.mtype = data[1];
            cmbuf.cmsg.type = data[1];
            cmbuf.cmsg.status = data[2];
#ifdef MSG_DEBUG
            printf("\nCTRLmsgInput: ");
            print_control_msg_buf(cmbuf);
#endif
            if (pmsgq_send(node_msqid, &cmbuf, sizeof(cmbuf), ACK, 2, 0)
                    == -1) {
#ifdef MSG_DEBUG
                perror("msgInput.pmsgq_send.Error al colocar en la cola de mensajes");
#endif
                return (-1);
            }
            break;
        default:

//            if (data[2] > MAX_MSG_DATA_LEN) {
//                return (-1);
//            }
            if ((size-3) > MAX_MSG_DATA_LEN) {
                return (-1);
            }

            cbuf.msg.options = data[0];
            cbuf.mtype = data[1];
            cbuf.msg.type = data[1];
            //cbuf.msg.len = data[2];
            cbuf.msg.len = size - 3;
            strcpy(cbuf.msg.nodeId, identifier);

            for (i = 0; i < cbuf.msg.len; i++) {
                //cbuf.msg.data[i] = data[3 + i];
                cbuf.msg.data[i] = data[2 + i];
            }
#ifdef MSG_DEBUG
            printf("\nmsgInput: ");
            print_msg_buf(cbuf);
#endif

            if (pmsgq_send(node_msqid, &cbuf, sizeof(cbuf), 0, 2, 0) == -1) {
            //if (pmsgq_send(node_msqid, &cbuf.msg, sizeof(struct msg_t), 0, 2, 0) == -1) {
#ifdef MSG_DEBUG
                perror("msgInput.pmsgq_send.Error al colocar en la cola de mensajes");
#endif
                return (-1);
            }
            break;
        }

        return 0;
    }
}

int sendPacket(struct msg_t *msg, uint16_t addr16, uint64_t addr64) {

    struct TX_status_qbuf status;
    memset(&status, 0, sizeof(struct TX_status_qbuf));
    int msqid = getTXSMsgq_id();
    uint8_t *curr_fid = (uint8_t*) pthread_getspecific(fid_var_key);

    if (*(curr_fid) == 0x7e || *(curr_fid) == 0x7d || *(curr_fid) == 0x11 || *(curr_fid) == 0x13) {
        *(curr_fid)+=1;
    }
    uint8_t fid = 0;
    if ((*(curr_fid) % 12)!=0 && (*(curr_fid) % 12)!=1) {
        fid = *(curr_fid) % 12; //TODO test this, still unstable
    }
    else {
        fid = 2;
    }


    if (addr16 != 0xfffe) {

        xbee_TxFrame16_t sending16;

        packetizeMsg_16(msg, &sending16, addr16);

        if (addr16 == 0xFFFF) {
            sending16.frameId = 0;
            if (packetOutput(&sending16, TX_REQUEST_16) == -1) {
#ifdef MSG_DEBUG
                perror("sendPacket.packetOutput.Error al enviar broadcast");
#endif
                return (-1);
            }
        } else {

            sending16.frameId = fid;

            if (packetOutput(&sending16, TX_REQUEST_16) == -1) {
#ifdef MSG_DEBUG
                perror("sendPacket:packetOutput. Error al enviar unicast16");
#endif
                return (-1);
            }

            if (pmsgq_receive(msqid, &status, sizeof(status), NULL, TX_STATUS_TIMEOUT, 0) == 0) {
                if ((status.tx_status.frameId != sending16.frameId)
                        || (status.tx_status.status != 0)) {
#ifdef MSG_DEBUG
                    fprintf(stderr,"sendPacket.No se recibió MACK\n");
#endif
                    return (-1);
                }
                *(curr_fid)+=1;
                return (0);
            } else {
#ifdef MSG_DEBUG
                perror("sendPacket:pmsgq_recv. Error al retirar status de la cola de mensajes");
#endif
            }
        }

        return (0);

    } else {
        xbee_TxFrame64_t sending64;

        packetizeMsg_64(msg, &sending64, addr64);

        if ((*(curr_fid) % 12)!=0) {
            sending64.frameId = *(curr_fid) % 12;  //TODO test this, still unstable
        }
        else {
            sending64.frameId = 1;
        }

        sending64.frameId = fid;

        if (packetOutput(&sending64, TX_REQUEST_64) == -1) {
#ifdef MSG_DEBUG
            perror("sendPacket:packetOutput. Error al enviar unicast64");
#endif
            return (-1);
        }

        if (pmsgq_receive(msqid, &status, sizeof(status), NULL, TX_STATUS_TIMEOUT, 0) == 0) {
            if ((status.tx_status.frameId != sending64.frameId)
                    || (status.tx_status.status != 0)) {
#ifdef MSG_DEBUG
                fprintf(stderr,"sendPacket. No se recibió MACK\n");
#endif
                return (-1);
            }
            *(curr_fid)+=1;
            return (0);
        } else {
#ifdef MSG_DEBUG
                perror("sendPacket:msgq_timed_recv. Error al retirar status de la cola de mensajes");
#endif
        }
    }
}

int msgMulticast(struct msg_t *msg, dest_list_t *list) {

    uint8_t i = 0, sent = 0, macked = 0;
    struct TX_status_qbuf status;

    struct timeval timer;

    if (msg == NULL || list == NULL) {
        errno = EINVAL;
        return (-1);
    }

    msg->options |= BROADCAST_BIT;

    for (i = 0; i < list->size; i++) {

        wpan_node_t *tmp = (wpan_node_t*) findNodeById(list->id_list[i]);
        if (tmp != NULL) {
            if (tmp->adress16 != 0xfffe) {

                xbee_TxFrame16_t sending16;

                packetizeMsg_16(msg, &sending16, tmp->adress16);
                sending16.frameId = 1;
                if (packetOutput(&sending16, TX_REQUEST_16) == -1) {
#ifdef MSG_DEBUG
                    fprintf(stderr,"msgMulticast:packetOutput. Error al enviar paquete16");
#endif
                    return (-1);
                }
                ++sent;
            } else {
                xbee_TxFrame64_t sending64;

                packetizeMsg_64(msg, &sending64, tmp->adress64);

                sending64.frameId = 1;

                if (packetOutput(&sending64, TX_REQUEST_64) == -1) {
#ifdef MSG_DEBUG
                    fprintf(stderr,"msgMulticast:packetOutput. Error al enviar paquete64");
#endif
                    return (-1);
                }
                ++sent;
            }

        } else {
            fprintf(stderr, "\nNode %s Not found.", list->id_list[i]);
        }
    }

    i = 0;

    gettimeofday(&timer, NULL);
    unsigned short int start = timer.tv_sec;
    unsigned short int start_ms = timer.tv_usec/1000;
    unsigned short int time = 0;

    do {

        if (pmsgq_receive(getTXSMsgq_id(), &status, sizeof(status), NULL, TX_STATUS_TIMEOUT, 0) < 0) {
#ifdef MSG_DEBUG
            perror("msgMulticast:msgq_timed_recv. Error al retirar status de la cola de mensajes");
#endif
        } else {
            i++;
            if ((status.tx_status.frameId != 1)
                    || (status.tx_status.status != 0)) {
#ifdef MSG_DEBUG
                fprintf(stderr,"msgMulticast. No se recibió MACK");
#endif
                //return (-1);
            } else {
                macked++;
            }
        }

        memset(&timer, 0, sizeof(struct timeval));
        gettimeofday(&timer, NULL);
        time = (timer.tv_sec - start)*1000 + ((timer.tv_usec/1000 - start_ms));
    //} while (i < (sent / 2));
    } while (i < (sent) && time < (TX_STATUS_TIMEOUT * (sent)));

    //if (macked < (sent / 2)) {
    if (macked < (sent)) {
#ifdef MSG_DEBUG
        fprintf(stderr,"msgMulticast. No se recibieron suficientes MACKs\n");
#endif
        return (-1);
    } else {

        return (0);
    }
}
